#!/usr/bin/env python3
import math
import os
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare


def generate_launch_description():
    package_path = get_package_share_directory("controller")
    rviz__file = os.path.join(package_path, "config", "path_view.rviz")

    odom_node = Node(
        package="controller",
        executable="odom",
        name="real_odom",
        output="screen",
    )

    follow_prediction_node = Node(
        package="controller",
        executable="follow_prediction",
        name="follow_prediction",
        output="screen",
        remappings=[("odom", "sp_odom")],
        parameters=[{"x_scale": 1.0}, {"y_scale": 10.0}, {"prediction_distance": 2.0}],
    )

    follow_mpc_node = Node(
        package="controller_py",
        executable="mpc_path_tracker",
        name="mpc_path_tracks",
        output="screen",
        # remappings=[("odom", "sp_odom")],
        parameters=[
            {"max_gyro": 30 / 180.0 * math.pi},
            {"mat_q": [10.0, 100.0, 10.0, 10.0]},
            {"mat_r": [1.0, 1.0]},
            {"vec": 0.2},
        ],
    )

    path = Node(
        package="path_planer",
        executable="path_publisher",
        name="path_publisher",
        output="screen",
        parameters=[{"path_file": "experiment/path_little2.csv"}, {"vec": 0.05}],
    )

    kinematic_robot = Node(
        package="robot_sim",
        executable="robot_kinematic_sim",
        name="kinematic_robot",
        output="screen",
        parameters=[{"max_speed": 5.0}],
    )

    dynamic_robot = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            [
                PathJoinSubstitution(
                    [
                        FindPackageShare("controller"),
                        "launch",
                        "dynamic_robot_with_controller.launch.py",
                    ]
                )
            ]
        )
    )

    real_robot = IncludeLaunchDescription(
        PythonLaunchDescriptionSource(
            [
                PathJoinSubstitution(
                    [
                        FindPackageShare("controller"),
                        "launch",
                        "real_robot_with_controller.launch.py",
                    ]
                )
            ]
        )
    )

    rviz2_node = Node(
        package="rviz2",
        executable="rviz2",
        name="rviz2",
        output="screen",
        arguments=["-d", rviz__file],
    )

    plot = Node(
        package="toolbox",
        executable="plot_runtime",
        name="plot_runtime",
        output="screen",
        parameters=[
            {
                "topics": "torque,rotate_torque,target_angle,angle,target_gyro,gyro,target_gyroz,gyroz,target_speed,speed"
            },
            {"auto_start": True},
        ],
    )

    return LaunchDescription(
        [
            # kinematic_robot,
            # dynamic_robot,
            real_robot,
            odom_node,
            # rviz2_node,
            path,
            # plot,
            # follow_prediction_node,
            follow_mpc_node,
        ]
    )
